//Here's the documentation for the AccelStepper file
//http://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html
#include <AccelStepper.h>
#include <iostream>
#include <string>
// include the LCD library code:
#include <Wire.h>
#include <LiquidCrystal.h>
#include <Servo.h> 
 
//==================================
// Dispensing Servo
//==================================
Servo myservo; 
int pos = 0;    // variable to store the servo position 

//==================================
// Dispensing Solenoid (pin)
//==================================
int SolenoidFour = 8;  // set pin for Solenoid One
int SolenoidThree = 7;  // set pin for Solenoid Two
int SolenoidTwo;  // set pin for Solenoid One
int SolenoidOne;  // set pin for Solenoid Two


//==================================
// Spirit Stations
//==================================
const long StationOne = -8000;
//distance: 8600
const long StationTwo = -16800;
//distance: 7450
const long StationThree = -23550;
//distance: 8800
const long StationFour = -32350;
//distance: 7900
const long StationFive = -40250;
//distance: 8250
const long StationSix = -48500;
//==================================
//==================================

//==================================
// Mixer Stations
//==================================
const long MixerSeven = -11800;
//distance: 8600
const long MixerEight = -19825;
//distance: 7450
const long MixerNine = -27950;
//distance: 8800
const long MixerTen = -36300;
//==================================
//==================================
const long StationHome = -1;


int Stopped = 1;


////////////////////////////////////////////////////////////////////////////////////////////////////////
//initialize Stepper Motor
////////////////////////////////////////////////////////////////////////////////////////////////////////
int motorSpeed = 9000; //maximum steps per second (about 3rps / at 16 microsteps)
int motorAccel = 9000; //steps/second/second to accelerate

int motorDirPin = 5; //digital pin 2
int motorStepPin = 4; //digital pin 3

int movementAmt;
int movementDir = 2;  //1 = right, 2 = left
//set up the accelStepper intance
//the "1" tells it we are using a driver
AccelStepper stepper(1, motorStepPin, motorDirPin); 

long Going = 0;

//========================================================================================================


//////////////////////////////////////////////////////////////////////////////
//Initialize Spirit Dispensing DC motors
//////////////////////////////////////////////////////////////////////////////
// http://learn.adafruit.com/adafruit-arduino-lesson-15-dc-motor-reversing/an-experiment

// Pin Layout  --   Left side 1,2,3,4,5,6,7,8, RIGHT SIDE: 9
// 1 = ENABLE PIN
// 2 = In1Pin
// 3 = DC Motor NEG
// 4 = Ground
// 5 = 
// 6 = DC motor POS
// 7 = in2Pin
// 8 = POWER
// 9 = POWER

int enablePin = 11;   //
int in1Pin = 10;
int in2Pin = 9;
boolean reverse;

//////////////////////////////////////////////////////////////////////////////
//Initialize Limit switch sensor  
//////////////////////////////////////////////////////////////////////////////


int limitSwitchSensorLeft = 5;
int limitSwitchSensorRight = 3;

volatile int RightSensor;   //
volatile int LeftSensor;    // 


//////////////////////////////////////////////////////////////////////////////
//Initialize Serial Communication variable
//////////////////////////////////////////////////////////////////////////////
char incomingByte;


//////////////////////////////////////////////////////////////////////////////
//Initialize Cup sensor
//////////////////////////////////////////////////////////////////////////////

int weightSensorPin = 4;        // select the input pin for the Weight Sensor
double weightThreshold = .10;   // select the cutoff for sensing a cup



void setup() {
  
  // Initialize Inturupt
  //attachInterrupt(1, IntLeftSwitch, RISING);   // 3 = LEFT
  attachInterrupt(1, IntRightSwitch, RISING);   // 2 = right
  
  pinMode( 2,INPUT);
  pinMode( 3,INPUT);
  
  pinMode(SolenoidOne,OUTPUT); //set pinMode for Solenoid One
  
  RightSensor = LOW;   //
  LeftSensor = LOW; 
  //pretend this is a serial communication:
//////////////////////  String serialComOne[3] = {"49000","1","2"};
//////////////////////  String serialComTwo[3] = {"35000","0","1"};
  
  //Setup stepper motor  
  stepper.setMaxSpeed(motorSpeed);
  stepper.setSpeed(motorSpeed);
  stepper.setAcceleration(motorAccel);
  

  // Setup DC motor for Dispensing
  pinMode(in1Pin, OUTPUT);
  pinMode(in2Pin, OUTPUT);
  pinMode(enablePin, OUTPUT);
  reverse = false;
  ////////////////////////////////

  myservo.attach(9);  // attaches the servo on pin 9 to the servo object 


  Serial.begin(9600);
  
  initializeServo();
  InitializeSteppermotor();
  initializeSolenoids(); 
    
}

void loop() {
  //int movementDir = 1;  //1 = right, 2 = left

  if (Serial.available() > 0) {
          // read the incoming byte:
          incomingByte = Serial.read();
          
          if (incomingByte >= '1' && incomingByte <= '6')
          {          
            // say what you got:
            //Serial.print("Going to Spirit Station: ");
            //Serial.println(incomingByte);
            
            GoToStation(incomingByte);
            delay(150);
            if(incomingByte % 2 == 0)
            {
              DispenseSpirit(1);
            }  
            else
            {
              DispenseSpirit(.5);
            }
        }
          else if (incomingByte >= '7' && incomingByte <= '9'|| incomingByte == '*')
          {
            //Serial.print("Going to Mixer Station: ");
            //Serial.println(incomingByte);
            
            GoToStation(incomingByte);
            
            DispenseMixer(incomingByte);   // argument is the delay time. 
            
          }
          else if (incomingByte == 'r')
          {
              //PourRumCoke();
            
          }
          else if (incomingByte == 'h')
          {
            //Serial.println("Going Home");            
            GoToStation('h');
          }
  }

}
boolean CheckIfSensesCup()
{    
    while( weightThreshold > analogRead(weightSensorPin) )
    {
        //Serial.println("Waiting for cup...");
        //UpdateState("Waiting for Cup");
        delay(500);           
    }
  
    //Serial.println("");
    //Serial.println("Cup registered");  
}



void GoToStation(char StationID)
{
  switch (StationID) {
    case 'h':
      //Serial.println("Moving - Home ");
      stepper.moveTo(-1);
      Going = -1;
      break;
    case '1':
      //Serial.println("Moving - 1");
      stepper.moveTo(StationOne);
      Going = StationOne;
      break;
    case '2':
      //Serial.println("Moving - 2");
      stepper.moveTo(StationTwo);
      Going = StationTwo;
      break;
    case '3':
      //Serial.println("Moving - 3");
      stepper.moveTo(StationThree);
      Going = StationThree;
      break;
    case '4':
      //Serial.println("Moving - 4");
      stepper.moveTo(StationFour);
      Going = StationFour;
      break;
    case '5':
      //Serial.println("Moving - 5");
      stepper.moveTo(StationFive);
      Going = StationFive;
      break;
    case '6':
      //Serial.println("Moving - 6");
      stepper.moveTo(StationSix);
      Going = StationSix;
      break;
    case '7':
      //Serial.println("Moving - 7");
      stepper.moveTo(MixerSeven);
      Going = MixerSeven;
      break;
    case '8':
      //Serial.println("Moving - 8");
      stepper.moveTo(MixerEight);
      Going = MixerEight;
      break;
    case '9':
      //Serial.println("Moving - 9");
      stepper.moveTo(MixerNine);
      Going = MixerNine;
      break;
    case '*':
      //Serial.println("Moving - 10");
      stepper.moveTo(MixerTen);
      Going = MixerTen;
      break;      
  }
  
  while (Going != stepper.currentPosition())
  {
    stepper.run();   
  }   
  
   stepper.moveTo(stepper.currentPosition());
   
   CompleteStep();
   
   //******************************************************
   /*
   Serial.print("Stopping at Station ");
   Serial.print(StationID);
   Serial.print(" - at position: ");
   Serial.println(stepper.currentPosition());
   Serial.println("--------------------------------");  
   //*****************************************************
   */
}
void CompleteStep()
{
   Serial.print("d");  //d = done
}

void InitializeSteppermotor()
{
  stepper.moveTo(320000);
  
  //Begin the startup process
  while(RightSensor == LOW && LeftSensor == LOW )
  {    
    stepper.run();   
  }
  
  stepper.setCurrentPosition(0); 
  stepper.moveTo(stepper.currentPosition());
  //stepperZero = stepper.currentPosition();
  
  /*
  Serial.print("Setting Zero to: ");
  Serial.println(stepper.currentPosition());
  Serial.println("");
  Serial.println("");
  */
  
  //moving away from switch
  stepper.moveTo(-1000);
  while (stepper.distanceToGo() > 0)
  {
    stepper.run();   
  }  
  stepper.moveTo(stepper.currentPosition());
  
  
  ResetInterruptVars(); 
}


void ResetInterruptVars()
{
    RightSensor = LOW;  
    LeftSensor = LOW; 
}

//Interrupts!!!!
void IntRightSwitch()
{
    RightSensor = HIGH;  
}
void IntLeftSwitch()
{
    LeftSensor = HIGH;  
}

void DispenseSpirit(int numShots)
{
  
   for(pos = 70; pos < 105; pos += 1)  // goes from 0 degrees to 180 degrees 
  {      
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(20);                       // waits 15ms for the servo to reach the position 
  }   
  
  delay(7000);
  
  /*
  switch (numShots)  //how long do we wait?
  {
    case '.5':
      delay(2500);
      break;
    case '1':
      delay(5000);
    break;    
  }
  */
  for(pos = 105; pos>=70; pos-=1)     // goes from 180 degrees to 0 degrees 
  {         
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(20);                       // waits 15ms for the servo to reach the position     
  }  
  CompleteStep();
}
void DispenseMixer(int charStation)
{
  int stationID = 0;
  
   switch (charStation) 
   {
    case '7':
      stationID = SolenoidOne;
      break;
    case '8':
      stationID = SolenoidTwo;
      break;
    case '9':
      stationID = SolenoidThree;
      break;
    case '*':
      stationID = SolenoidFour;
      break;   
  }
  
  if(stationID != 0)
  {
    /*
    digitalWrite(stationID,HIGH);
    delay(3000);
    digitalWrite(stationID,LOW); 
    delay(2000); 
    CompleteStep();
    */
  }
}

void initializeServo()
{
   for(pos = 50; pos>=1; pos-=1)     // goes from 180 degrees to 0 degrees 
  {         
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(20);                       // waits 15ms for the servo to reach the position 
    
  }   
}
void initializeSolenoids()
{
    digitalWrite(SolenoidFour,LOW);     
    digitalWrite(SolenoidThree,LOW);  
    /*
    digitalWrite(SolenoidThree,LOW);  
    digitalWrite(SolenoidFour,LOW);   
    */
}

void PourRumCoke()
{
  GoToStation('2');
  delay(150);
  DispenseSpirit(1);
  delay(1500);
  GoToStation('*');
  delay(150);
  DispenseMixer(10);

  GoToStation('h');
}

